#include <SoftwareSerial.h>
#include <math.h>
#include <Arduino.h>
#include <MemoryFree.h>
#define rxPin 43
#define txPin 36
SoftwareSerial smcSerial = SoftwareSerial(rxPin, txPin);
const int frontLeft  =1;
const int rearLeft   =2;
const int frontRight =3;
const int rearRight  =4;

//Serial1 communicates with pin 19(RX) and pin 18 (TX) this controls the left motors
//Serial2 communicates with pins 17(RX) and 16(TX) this controls the right motors

int numOfMotors=4;
//change to number of attached motors 

//------------------------------------------>setup<---------------------------------------------------------------------
void setup()
{
  // initialize software serial object with baud rate of 19.2 kbps
  Serial1.begin(19200);
  Serial2.begin(19200);
  Serial3.begin(19200);
  smcSerial.begin(19200);
  Serial.begin(9600);
  pinMode(5, OUTPUT);                                                   // CHANGED
  digitalWrite(5, LOW);
  pinMode(5,INPUT);
  
  pinMode(6, OUTPUT);                                                   // CHANGED
  digitalWrite(6, LOW);
  pinMode(6,INPUT);
  
  pinMode(7, OUTPUT);
  digitalWrite(7, LOW);
  pinMode(7, INPUT);
 
  pinMode(8, OUTPUT);                                                   // CHANGED
  digitalWrite(8, LOW);
  pinMode(8,INPUT);
  
  // the Simple Motor Controller must be running for at least 1 ms
  // before we try to send serial data, so we delay here for 5 ms
  delay(5);
 
  // if the Simple Motor Controller has automatic baud detection
  // enabled, we first need to send it the byte 0xAA (170 in decimal)
  // so that it can learn the baud rate
  Serial1.write(0xAA);  // send baud-indicator byte
  delay(5);
  Serial2.write(0xAA);  // send baud-indicator byte
  delay(5);
  Serial3.write(0xAA);
  delay(5);
  smcSerial.write(0xAA);
  // next we need to send the Exit Safe Start command, which
  // clears the safe-start violation and lets the motor run
  exitSafeStart();  // clear the safe-start violation and let the motor run
}
//----------------------------------------------------------------------------------------------------------------------
//-------------------------------------->exitSafeStart<-----------------------------------------------------------------
void exitSafeStart()
{
    Serial1.write(0xAA);
    delay(5);
    Serial1.write(0x83);
    delay(5);
    Serial2.write(0xAA);
    delay(5);
    Serial2.write(0x83);
    delay(5);
    Serial3.write(0xAA);
    delay(5);
    Serial3.write(0x83);
    delay(5);
    smcSerial.write(0xAA);
    delay(5);
    smcSerial.write(0x83);
}
//----------------------------------------------------------------------------------------------------------------------
//-------------------------------------->setMotorSpeed<-----------------------------------------------------------------
void setMotorSpeed(int speed, const int deviceNumber)
{
 // speed should be a number from -3200 to 3200
 if(deviceNumber==1){
    Serial1.write(0xAA);
    delay(5);
    if (speed < 0)
    {
      speed= speed*-1;
      Serial1.write(0x86); //motor reverse command
    }
    else
    {
      Serial1.write(0x85);  // motor forward command
    }
    delay(5);
    Serial1.write(speed & 0x1F);
    delay(5);
    Serial1.write(speed >> 5);
 }else if(deviceNumber==2){
    Serial2.write(0xAA);
    delay(5);
    if (speed < 0)
    {
      speed= speed*-1;
      Serial2.write(0x86); //motor reverse command
    }
    else
    {
      Serial2.write(0x85);  // motor forward command
    }
    delay(5);
    Serial2.write(speed & 0x1F);
    delay(5);
    Serial2.write(speed >> 5);  
  }else if(deviceNumber==3){
    Serial3.write(0xAA);
    delay(5);
    if (speed < 0)
    {
      speed= speed*-1;
      Serial3.write(0x86); //motor reverse command
    }
    else
    {
      Serial3.write(0x85);  // motor forward command
    }
    delay(5);
    Serial3.write(speed & 0x1F);
    delay(5);
    Serial3.write(speed >> 5);  
  }else if(deviceNumber==4){
    smcSerial.write(0xAA);
    delay(5);
    if (speed < 0)
    {
      speed= speed*-1;
      smcSerial.write(0x86); //motor reverse command
    }
    else
    {
      smcSerial.write(0x85);  // motor forward command
    }
    delay(5);
    smcSerial.write(speed & 0x1F);
    delay(5);
    smcSerial.write(speed >> 5);  
  }
}
//--------------------------------------->Read_Status<------------------------------------------------------------------
//Added- not tested
void read_status()
{
  int error;
  int targetSpeed;
  int currentSpeed;
  int voltage;
  int numOfDevices=1;
  for(numOfDevices; numOfDevices <= numOfMotors; numOfDevices++){
   error=getVariable(0,numOfDevices);
   delay(5);
   targetSpeed= getVariable(20,numOfDevices); 
   delay(5);
   currentSpeed=getVariable(21,numOfDevices);
   delay(5);
   voltage= getVariable(23,numOfDevices);
   delay(5);

   Serial.print('|');
   Serial.print(error, DEC);
   Serial.print('|');
   Serial.print(targetSpeed, DEC);
   Serial.print('|');
   Serial.print(currentSpeed, DEC);
   Serial.print('|');
   Serial.print(voltage, DEC);
   Serial.print('|');
   Serial.print('/');
    /*
   Serial.print('|');
   Serial.print(160000, DEC);
   Serial.print('|');
   Serial.print(3200, DEC);
   Serial.print('|');
   Serial.print(3200, DEC);
   Serial.print('|');
   Serial.print(12609, DEC);
   Serial.print('|');
   Serial.print('/');
   */
  }
}
//----------------------------------------------------------------------------------------------------------------------
//--------------------------------------->getVariable<------------------------------------------------------------------
signed int getVariable(int variableID, int devicenum)
{
  int value;
  if(devicenum==1){
  Serial1.write(0xA1);
  delay(4);
  Serial1.write(variableID);
  delay(4);
  value  = Serial1.read();
  delay(4);
  value= value +256*Serial1.read();
  }else if(devicenum==2){
  Serial2.write(0xA1);
  delay(4);
  Serial2.write(variableID);
  delay(4);
  value  = Serial2.read();
  delay(4);
  value= value +256*Serial2.read();
  }else if(devicenum==3){
  Serial3.write(0xA1);
  delay(4);
  Serial3.write(variableID);
  delay(4);
  value  = Serial3.read();
  delay(4);
  value= value +256*Serial3.read();  
  return value;
  }else if(devicenum==4){
  smcSerial.write(0xA1);
  delay(4);
  smcSerial.write(variableID);
  delay(4);
  value = smcSerial.read();
  delay(4);
  value= value +256*smcSerial.read();
  } 
  return value;
}
//----------------------------------------------------------------------------------------------------------------------

//--------------------------------------->stopMotor<--------------------------------------------------------------------
//TEST----------------------------------------------
void stopMotor(){
  Serial1.write(0xE0);
  delay(5);
  Serial2.write(0xE0);
  delay(5);
  Serial3.write(0xE0);
  delay(5);
  smcSerial.write(0xE0);
  delay(5);
}
//----------------------------------------------------------------------------------------------------------------------

void loop(){
  int val; // 0 to 9 
  int RorL; //Right or Left
  int NorP;
  float speedPlaceHolder; //reads in value of speed byte by byte
  float totalSpeed = 0; //the total speed to be set to a motor;
  boolean error = false;

  if(Serial.available()){
    Serial.println(freeMemory());
    val= Serial.read();
   if(val== 'M'){
      delay(10);
    while(Serial.available() >0){
      totalSpeed = 0;
      RorL=Serial.read();
      NorP=Serial.read();
        for(int count=3;  count >= 0; count--){
         speedPlaceHolder = Serial.read()- '0';
         if(speedPlaceHolder < 0 || speedPlaceHolder > 9){
           Serial.print(speedPlaceHolder);
           error = true;
           break;                                                                 //CHANGED
         }
           totalSpeed+=(pow(10,count))*speedPlaceHolder;
        }
        
      if(NorP == '-'){
        totalSpeed = totalSpeed*-1;
      }
      if(totalSpeed >= 3200){
        totalSpeed = 3200;
      }
      if(totalSpeed <= -3200){
        totalSpeed = -3200; 
      }
    //IF A RANDOM OR MISPLACED BIT OCCURS READ IN EXCESS DATA?
    //INSERT IF GREATER THAN 3200 STOP
      if(!error){                                                //Changed 3/3
        if(RorL == 'L'){
          setMotorSpeed(totalSpeed, frontLeft);
          delay(5);
          setMotorSpeed(totalSpeed, rearLeft);
          delay(5);
          Serial.print("Left Wheels ");
          Serial.println(totalSpeed);
        }else if(RorL == 'R'){
          Serial.print("Right Wheels  ");
          Serial.println(totalSpeed);
          setMotorSpeed(totalSpeed, frontRight);
          delay(5);
          setMotorSpeed(totalSpeed, rearRight);
          delay(5);
        }
      }else{
       Serial.print("Error Found! Rover Stopping!");
       setMotorSpeed(0, frontLeft);
        delay(5);
        setMotorSpeed(0, frontRight);
        delay(5);
        setMotorSpeed(0, rearLeft);
        delay(5);
        setMotorSpeed(0, rearRight);
        delay(5);
        break;
        delay(1000);
        Serial.flush();
        Serial1.flush();
        Serial2.flush();
        Serial3.flush();
        smcSerial.flush();
      }
   }
   }else if(val == 'S'){
      read_status();
   }else if(val == 'E'){

      Serial1.write(0xAA);  // send baud-indicator byte
      delay(5);
      Serial2.write(0xAA);
      delay(5);
      Serial3.write(0xAA);
      delay(5);
      smcSerial.write(0xAA);
      Serial.println("Exit safe start");
      exitSafeStart();
   }else if(val == 'K'){                                                      //ADDED 2/21
     stopMotor();
   }else if(val == 'X'){
      pinMode(5, OUTPUT);                                                   // ADDED 2/21
      digitalWrite(5, LOW);
      pinMode(5,INPUT);
      delay(5);
      pinMode(6, OUTPUT);
      digitalWrite(6, LOW);
      pinMode(6, INPUT);
      delay(5);
      pinMode(7, OUTPUT);                                                   // ADDED 2/21
      digitalWrite(7, LOW);
      pinMode(7,INPUT);
      delay(5);
      pinMode(8, OUTPUT);
      digitalWrite(8, LOW);
      pinMode(8, INPUT);
      delay(5);
   }
   
  }
}
